#include <tinyhal.h>
#include "RGBLEDMatrixAF.h"
const unsigned char* Font_GetGlyph16(unsigned short u);

#define RGBLMAF_WX_DEF  32
#define RGBLMAF_WY_DEF  16

#define COLOR_MONO  0
#define COLOR_64    1

// D2 P06   pinCK
// D3 P05   pinXR
// D4 P39   pinXB
// D5 P3A   pinXG
// D6 P3C   pinR2
// D7 P08   pinR1
// D8 P3E   pinR0
// D9 P3B   pinEN

static GPIO_PIN pinR1;
static GPIO_PIN pinG1;
static GPIO_PIN pinB1;
static GPIO_PIN pinR2;
static GPIO_PIN pinG2;
static GPIO_PIN pinB2;
static GPIO_PIN pinCK;
static GPIO_PIN pinEN;
static GPIO_PIN pinA;
static GPIO_PIN pinB;
static GPIO_PIN pinC;
static GPIO_PIN pinLAT;
static UINT8 cnt64;

#define PIN_L   0
#define PIN_H   1

static INT32 rgblmaf_sx = 0;
static INT32 rgblmaf_sy = 0;
static INT32 rgblmaf_ex = RGBLMAF_WX_DEF - 1;
static INT32 rgblmaf_ey = RGBLMAF_WY_DEF - 1;

static inline INT32 _abs(INT32 x)
{
    if (x < 0)
        return -x;
    else
        return x;
}

static inline INT32 sign(INT32 x)
{
    if (x < 0)
        return -1;
    else
        return 1;
}

void RGBLMAF_Initialize(GPIO_PIN* pins)
{
	UINT8 i;
	pinR1 = pins[0];
	pinG1 = pins[1];
	pinB1 = pins[2];
	pinR2 = pins[3];
	pinG2 = pins[4];
	pinB2 = pins[5];
	pinCK = pins[6];
	pinEN = pins[7];
	pinA = pins[8];
	pinB = pins[9];
	pinC = pins[10];
	pinLAT = pins[11];
	for (i = 0; i < 12; i++)
		CPU_GPIO_EnableOutputPin(pins[i], PIN_L);
    cnt64 = 0;
}

void RGBLMAF_SelectLine(UINT8 n)
{
    CPU_GPIO_SetPinState(pinA, (n & 1)? 1:0);
    CPU_GPIO_SetPinState(pinB, (n & 2)? 1:0);
    CPU_GPIO_SetPinState(pinC, (n & 4)? 1:0);

}

void RGBLMAF_TurnOn()
{
    CPU_GPIO_SetPinState(pinEN, PIN_H);
}

void RGBLMAF_TurnOff()
{
    CPU_GPIO_SetPinState(pinEN, PIN_L);
}

void RGBLMAF_DrawLine_Color2(UINT32 pat1, UINT8 *fcol1, UINT8 *bcol1, UINT32 pat2, UINT8 *fcol2, UINT8 *bcol2)
{
    UINT8 i, j;
    UINT8 v, f1, b1, f2, b2;
	CPU_GPIO_SetPinState(pinLAT, PIN_H);
    for (i = 0; i < RGBLMAF_WX_DEF; i++, pat1 >>= 1, pat2 >>=1) {
    	f1 = *fcol1++;
    	b1 = *bcol1++;
    	f2 = *fcol2++;
    	b2 = *bcol2++;
        if (pat2 & 0x01) {
        	CPU_GPIO_SetPinState(pinR1, (f1 & 1)? 0:1);
        	CPU_GPIO_SetPinState(pinG1, (f1 & 2)? 0:1);
        	CPU_GPIO_SetPinState(pinB1, (f1 & 4)? 0:1);
        } else {
        	CPU_GPIO_SetPinState(pinR1, (b1 & 1)? 0:1);
        	CPU_GPIO_SetPinState(pinG1, (b1 & 2)? 0:1);
        	CPU_GPIO_SetPinState(pinB1, (b1 & 4)? 0:1);
        }
        if (pat1 & 0x01) {
        	CPU_GPIO_SetPinState(pinR2, (f2 & 1)? 0:1);
        	CPU_GPIO_SetPinState(pinG2, (f2 & 2)? 0:1);
        	CPU_GPIO_SetPinState(pinB2, (f2 & 4)? 0:1);
        } else {
        	CPU_GPIO_SetPinState(pinR2, (b2 & 1)? 0:1);
        	CPU_GPIO_SetPinState(pinG2, (b2 & 2)? 0:1);
        	CPU_GPIO_SetPinState(pinB2, (b2 & 4)? 0:1);
        }
        CPU_GPIO_SetPinState(pinCK, PIN_H);
        CPU_GPIO_SetPinState(pinCK, PIN_L);
    }
	CPU_GPIO_SetPinState(pinLAT, PIN_L);
}

void RGBLMAF_WaitLine(volatile UINT32 loop)
{
    RGBLMAF_TurnOn();
    while (loop-- > 0) ;
    RGBLMAF_TurnOff();
}

void RGBLMAF_DrawScreen_Color2(UINT32 *pat1, UINT32 off1, UINT8 *fcol1, UINT8 *bcol1, UINT32 *pat2, UINT32 off2, UINT8 *fcol2, UINT8 *bcol2, UINT32 loop)
{
	UINT8 i;

	pat1 += off1;
	pat2 += off2;
    for (i = 0; i < 8; i++) {
        RGBLMAF_SelectLine(i);
        RGBLMAF_DrawLine_Color2(*pat1++, fcol1, bcol1, *pat2++, fcol2, bcol2);
        RGBLMAF_WaitLine(loop);
	}
}

static void RGBLMAF_UpdateCount()
{
    if (++(cnt64) >= 3)
        cnt64 = 0;
}

static UINT8 RGBLMAF_GetColor8(UINT8 col64)
{
    UINT8 col8 = 0;
    if (cnt64 < ((col64 >> 0) & 3))
        col8 |= 1;
    if (cnt64 < ((col64 >> 2) & 3))
        col8 |= 2;
    if (cnt64 < ((col64 >> 4) & 3))
        col8 |= 4;
    return col8;
}

void RGBLMAF_DrawPattern64(UINT8 *pat1, UINT8 *pat2)
{
    UINT8 i;
    UINT8 col1_8, col2_8;

    CPU_GPIO_SetPinState(pinLAT, PIN_H);
    for (i = 0; i < RGBLMAF_WX_DEF; i++, pat1++, pat2++) {
        col1_8 = RGBLMAF_GetColor8(*pat1);
        col2_8 = RGBLMAF_GetColor8(*pat2);
        CPU_GPIO_SetPinState(pinR1, (col2_8 & 1)? 0:1);
        CPU_GPIO_SetPinState(pinG1, (col2_8 & 2)? 0:1);
        CPU_GPIO_SetPinState(pinB1, (col2_8 & 4)? 0:1);
        CPU_GPIO_SetPinState(pinR2, (col1_8 & 1)? 0:1);
        CPU_GPIO_SetPinState(pinG2, (col1_8 & 2)? 0:1);
        CPU_GPIO_SetPinState(pinB2, (col1_8 & 4)? 0:1);
        CPU_GPIO_SetPinState(pinCK, PIN_H);
        CPU_GPIO_SetPinState(pinCK, PIN_L);
    }
    CPU_GPIO_SetPinState(pinLAT, PIN_L);
}

#if 0
void RGBLMAF_DrawLine64(UINT8 pattern, UINT8 fcol, UINT8 bcol)
{
    RGBLMAF_DrawLine8(pattern, RGBLMAF_GetColor8(fcol), RGBLMAF_GetColor8(bcol));
}
#endif

void RGBLMAF_DrawScreen_Color64(UINT8 *pat1, UINT32 off1, UINT8 *pat2, UINT32 off2, UINT32 loop)
{
    UINT8 i, j;
    UINT8 *p1, *p2;

    for (j = 0; j < 3; j ++) {
        p1 =  pat1 + off1;
        p2 =  pat2 + off2;
        for (i = 0; i < 8; i++) {
            RGBLMAF_SelectLine(i);
            RGBLMAF_DrawPattern64(p1, p2);
            RGBLMAF_WaitLine(loop);
            p1 += RGBLMAF_WX_DEF;
            p2 += RGBLMAF_WX_DEF;
        }
        RGBLMAF_UpdateCount();
    }
}

void RGBLMAF_GetFontBitmap(UINT16 unicode, UINT8 *fb, UINT8 bytes)
{
	UINT8 *pfont;
	UINT32 i;

	if (unicode < 0x80) {
		pfont = (UINT8 *)Font_GetGlyph((unsigned char)unicode);
	} else {
		pfont = (UINT8 *)Font_GetGlyph16((unsigned short)unicode);
	}
	if (pfont != NULL) {
		memcpy((void *)fb, (const void *)pfont, (size_t)bytes);
	} else {
		memset((void *)fb, 0, (size_t)bytes);
	}
}

// 32x16
// Int32[16]
//
// *buf[3]* *buf[2]* *buf[1]* *buf[0]* Int32 0
// ******** ******** ******** ******** Int32 1
// ******** ******** ******** ******** Int32 2
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ********
// ******** ******** ******** ******** Int32 15

//  Char0    Char1    Char2    Char3
//  Char4    Char5    Char6    Char7

static UINT8 bmask[] = {
    0xff,
    0xfe,
    0xfc,
    0xf8,
    0xf0,
    0xe0,
    0xc0,
    0x80
};

static UINT8 space[] = {
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00,
        0x00
};

#define WX  32
#define WY  16
#define DX  ((WX-1)/8)

static UINT8 *RGBLMAF_CalcBuf(UINT8 *buf, INT32 x)
{
    if (x < 0)
        buf -= (_abs(x/8) % 4);
    else
        buf += DX - ((x/8) % 4);
    return buf;
}

void RGBLMAF_SetPatToBuf(UINT8 *buf, INT32 sx, INT32 sy, UINT8 *pat, INT32 pwx, INT32 pwy)
{
    INT32 i, j, k;
    INT32 w, wx, wy;
    INT32 shift;
    UINT8 *pbuf;
    UINT8 m;
    if (sx < 0)
        shift = 8 - (_abs(sx) % 8);
    else
        shift = sx % 8;
    m = bmask[pwx % 8];

    if (sy + pwy > WY)
        wy = WY - sy;
    else
        wy = pwy;
    if (sx + pwx > WX)
        wx = WX - sx;
    else
        wx = pwx;
    buf += (WX/8)*sy;
    for (j = sy; j < sy + wy; j++) {
        if (j >= 0) {
            w = wx;
            k = 0;
            for (i = 0; i < (wx + 7)/8; i++, w-=8) {
                k = sx + i*8;
                if (k >= 0) {
                    pbuf = RGBLMAF_CalcBuf(buf, k);
                    *pbuf = ((*pbuf) & ~(m >> shift)) | ((*(pat + i)) >> shift);
                }
                if (k >= -1) {
                    pbuf = RGBLMAF_CalcBuf(buf, k + 8);
                    if ((shift != 0) && (shift + w > 8)) {
                        *pbuf = ((*pbuf) & ~(m << (8 - shift))) | ((*(pat + i)) << (8 - shift));
                    }
                }
            }
        }
        buf += (WX/8);
        pat += ((pwx+7)/8);
    }
}

void RGBLMAF_SetFontsToBuf(UINT16 *unicode, UINT8 *buf)
{
	UINT8 *pfont;
	INT32 x, y, wx, wy;
	BOOL bDB;
	wy = Font_Height();
	x = 0;
	y = 0;
	while (1) {
	    bDB = FALSE;
	    if (*unicode < 0x80) {
	        pfont = (UINT8 *)Font_GetGlyph((unsigned char)*unicode);
	        wx = Font_Width();
	        RGBLMAF_SetPatToBuf(buf, x, y, pfont, wx, wy);
	    } else {
	        pfont = (UINT8 *)Font_GetGlyph16((unsigned short)*unicode);
	        if (pfont == 0) {
	            pfont = (UINT8 *)&space;
	        }
	        wx = Font_Width() * 2;
	        RGBLMAF_SetPatToBuf(buf, x, y, pfont, wx, wy);
	        if (x + wx > WX) {
	            bDB = TRUE;
	        }
	    }
	    x += wx;
	    if (x >= WX) {
	        x -= WX;
	        y += wy;
	    }
	    if (y >= WY)
	        break;
	    if (bDB == TRUE)
            RGBLMAF_SetPatToBuf(buf, x-wx, y, pfont, wx, wy);
	    unicode++;
	}
}

void RGBLMAF_RotateXBuf8(UINT32 *buf, INT32 rx)
{
    UINT32 i;
    UINT32 a, b;
    INT32 sx;
    for (i = 0; i < 8; i++) {
        a = *buf;
        b = *(buf+8);
        if (rx > 0) {
            *buf = (b << (RGBLMAF_WX_DEF-rx)) | (a >> rx);
            *(buf+8) = (a << (RGBLMAF_WX_DEF-rx)) | (b >> rx);
        } else {
            sx = -rx;
            *buf = (b >> (RGBLMAF_WX_DEF-sx)) | (a << sx);
            *(buf+8) = (a >> (RGBLMAF_WX_DEF-sx)) | (b << sx);
        }
        buf++;
    }
}

void RGBLMAF_Pset_Mono(UINT8 *buf, INT32 x, INT32 y)
{
    UINT32 *tbuf = (UINT32 *)buf;
    tbuf[y] |= (1 << x);
}

void RGBLMAF_Pset_Color64(UINT8 *buf, INT32 x, INT32 y, UINT8 col)
{
    buf[x + y*RGBLMAF_WX_DEF] = col;
}

void RGBLMAF_Pset(UINT8 *buf, INT32 x, INT32 y, UINT8 col_type, UINT8 col)
{
    if ((x >= rgblmaf_sx) && (x <= rgblmaf_ex) && (y >= rgblmaf_sy) && (y <= rgblmaf_ey)) {
        if (col_type == COLOR_64)
            RGBLMAF_Pset_Color64(buf, x, y, col);
        else
            RGBLMAF_Pset_Mono(buf, x, y);
    }
}

void RGBLMAF_Line(UINT8 *buf, INT32 x1, INT32 y1, INT32 x2, INT32 y2, UINT8 col_type, UINT8 col)
{
    INT32 x;
    INT32 y;
    INT32 dx;
    INT32 dy;
    INT32 s1;
    INT32 s2;
    INT32 temp;
    INT32 interchange;
    INT32 e;
    INT32 i;

    x = x1;
    y = y1;
    dx = _abs(x2 - x1);
    dy = _abs(y2 - y1);
    s1 = sign(x2 - x1);
    s2 = sign(y2 - y1);

    if (dy > dx) {
        temp = dx;
        dx = dy;
        dy = temp;
        interchange = 1;
    } else
        interchange = 0;
    e = 2 * dy - dx;
    for (i = 0; i <= dx; i++) {
        RGBLMAF_Pset(buf, x, y, col_type, col);
        while (e >= 0) {
            if (interchange == 1)
                x += s1;
            else
                y += s2;
            e -= 2*dx;
        }
        if (interchange == 1)
            y += s2;
        else
            x += s1;
        e += 2 * dy;
    }
}

void RGBLMAF_Circle(UINT8 *buf, INT32 x, INT32 y, INT32 r, UINT8 col_type, UINT8 col)
{
    INT32 xi = 0;
    INT32 yi = r;
    INT32 di = 2 * (1 - r);
    INT32 limit = 0;
    INT32 delta1;
    INT32 delta2;

    while (1) {
circle1:
        RGBLMAF_Pset(buf, x+xi, y+yi, col_type, col);
        RGBLMAF_Pset(buf, x-xi, y+yi, col_type, col);
        RGBLMAF_Pset(buf, x+xi, y-yi, col_type, col);
        RGBLMAF_Pset(buf, x-xi, y-yi, col_type, col);
        if (yi <= limit)
            goto circle4;
        if (di < 0)
            goto circle2;
        if (di > 0)
            goto circle3;
        //if (di == 0)
         goto circle20;
circle2:
        delta1 = 2 * di + 2 * yi - 1;
        if (delta1 <= 0)
            goto circle10;
        //if (delta > 0)
            goto circle20;
circle3:
        delta2 = 2 * di - 2 * xi - 1;
        if (delta2 <= 0)
            goto circle20;
        //if (delta2 > 0)
            goto circle30;
circle10:
        xi += 1;
        di += (2 * xi + 1);
        continue;   // goto circle1;
circle20:
        xi += 1;
        yi -= 1;
        di += (2 * xi - 2 * yi + 2);
        continue;   // goto circle1;
circle30:
        yi -= 1;
        di -= (2 * yi - 1);
        continue;   // goto circle1;
    }
    circle4:
    return;
}
